/* commands_at91sam7.c -- transport agnostic utilities for AT91SAM7*
**
** Copyright 2006, Brian Swetland.  All rights reserved.
** See provided LICENSE file or http://frotz.net/LICENSE for details.
*/

#include <string.h>
#include "debugger.h"


int
cmd_setclk() // AT91SAM7: clock to main osc (18.432MHz)
{
    int timeout;
    halt();
    
    wr32(0xfffffc20, 0x00000000); /* reset MOSCS */
    wr32(0xfffffc20, 0x00004001); /* enable main oscillator */

    for(timeout = 0; timeout < 100; timeout++){
        if(rd32(0xfffffc68) & 1) goto okay1;
    }
    return -1;

okay1:
    wr32(0xfffffc30, 0x00000001); /* MCK = main osc / 1 */
    
    for(timeout = 0; timeout < 100; timeout++){
        if(rd32(0xfffffc68) & 8) return 0;
    }
    return -1;
}



#define MC_FMR       0xffffff60
#define MC_FCR       0xffffff64
#define MC_FSR       0xffffff68

#define MC_FRDY      0x00000001
#define MC_LOCKE     0x00000004
#define MC_PROGE     0x00000008
#define MC_SECURITY  0x00000010
#define MC_GPNVM0    0x00000100
#define MC_GPNVM1    0x00000200

#define MC_NEBP      0x00000080
#define MC_FWS_1     0x00000000
#define MC_FWS_2     0x00000100
#define MC_FWS_3     0x00000200
#define MC_FWS_4     0x00000300
#define MC_FMCN(n)   (((n) & 0xff) << 16)

#define MC_PAGEN(n)  (((n) & 0x3ff) << 8)
#define MC_KEY       0x5a000000
#define MC_CMD_NONE  0x00000000
#define MC_CMD_WP    0x00000001
#define MC_CMD_SLB   0x00000002
#define MC_CMD_WPL   0x00000003
#define MC_CMD_CLB   0x00000004
#define MC_CMD_EA    0x00000008
#define MC_CMD_SGPB  0x0000000b
#define MC_CMD_CGPB  0x0000000d
#define MC_CMD_SSB   0x0000000f

/* BUG: adjust this based on product ID */
#define FLASH_BOTTOM     0x00100000
#define FLASH_SIZE       (64*1024)
#define FLASH_TOP        (FLASH_BOTTOM + FLASH_SIZE)
#define FLASH_PAGE_SIZE  128
#define FLASH_PAGE_MASK  (FLASH_PAGE_SIZE - 1)
#define FLASH_LOCK_BITS  16

static void
flash_command(unsigned cmd, unsigned pagen)
{
    unsigned n;

    while(!(rd32(MC_FSR) & MC_FRDY));
    wr32(MC_FCR, MC_PAGEN(pagen) | cmd | MC_KEY);
    for(;;) {
        n = rd32(MC_FSR);
        if(n & MC_PROGE) {
            ERROR("flash programming error");
            break;
        }
        if(n & MC_LOCKE) {
            ERROR("flash lock error");
            break;
        }
        if(n & MC_FRDY) break;
    }
}

void
cmd_unlock() // AT91SAM7: clear the flash lock bits
{
    unsigned n, lockbits;

    lockbits = rd32(MC_FSR) >> 16;
    
        /* BUG: hardcoded timing for 18MHz */
    wr32(MC_FMR, 0x00050100);

    for(n = 0; n < FLASH_LOCK_BITS; n++){
        if(lockbits & 1) {
            msg("unlocking lock bit #%d\n", n);
            flash_command(MC_CMD_CLB,
                          (n * (FLASH_SIZE / FLASH_LOCK_BITS)) / FLASH_PAGE_SIZE);
        }
        lockbits >>= 1;
    }
}

void
cmd_flash(Address addr, String fn) // AT91SAM7: write a file to flash
{
    int size;
    void *data;
    unsigned *ptr;
    unsigned readback[512];
    
    data = load_file(fn, &size);
    size = (size + FLASH_PAGE_MASK) & (~FLASH_PAGE_MASK);
    
    if(data == 0) {
        ERROR("cannot load file '%s'", fn);
        return;
    }    
    if(addr & 127) {
        ERROR("not page aligned");
        return;
    }
    if((addr < FLASH_BOTTOM) || (addr > FLASH_TOP)) {
        ERROR("address out of range");
        return;
    }
    if((addr + size) > FLASH_TOP) {
        ERROR("image will not fit");
        return;
    }

        /* BUG: hardcoded timing for 18MHz */
    wr32(MC_FMR, 0x00340000);
    if(failed()) return;
    
    msg("writing %d bytes to flash @ 0x%08x\n", size, addr);
    ptr = (unsigned*) data;
    while(size > 0){
        wr32x(addr, ptr, FLASH_PAGE_SIZE / 4);
        if(failed()) return;
        flash_command(MC_CMD_WP, (addr & (FLASH_SIZE - 1)) / FLASH_PAGE_SIZE);
        if(failed()) return;
#if 1
        rd32x(addr, readback, FLASH_PAGE_SIZE / 4);
        if(failed()) return;
        if(memcmp(readback, ptr, FLASH_PAGE_SIZE)) {
            ERROR("verify error");
            return;
        }
#endif
        size -= FLASH_PAGE_SIZE;
        addr += FLASH_PAGE_SIZE;
        ptr += (FLASH_PAGE_SIZE / 4);
    }
}

extern void cmd_reset();

void
cmd_run(String fn) // AT91SAM7: download to 0x202000 and run
{
    int size;
    void *data;
    unsigned addr;
    
    data = load_file(fn, &size);
    size = (size + 3) & (~3);
    addr = 0x00202000;
    
    if(data == 0) {
        ERROR("cannot load file '%s'", fn);
        return;
    }

    invoke("reset", "-");
    
    msg("writing %d bytes to ram @ 0x%08x\n", size, addr);
    wr32x(addr, (unsigned*) data, size);
    
    msg("executing code at 0x%08x\n", addr);
    jump_to(addr);
}

